/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, LABUST, UNIZG-FER
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the LABUST nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Author : Dula Nad
 *  Created: 01.02.2013.
 *********************************************************************/
#ifndef BACKWARDCOMPATIBILITY_HPP_
#define BACKWARDCOMPATIBILITY_HPP_

#include "../ConfigXMLParser.h"
#include "../ROSInterface.h"

#include <boost/preprocessor/seq.hpp>

namespace plugin
{
	/**
	 * This class is used to avoid writing extra code for testing of suggested sensor abstractions.
	 */
	template <
	  class Sensor,
	  class XMLConfig,
	  class ROSIface>
	class BackwardCompatibleSensorFactoryImpl : public virtual SensorInterface
	{
		typedef void (*XMLConfigurer)(const xmlpp::Node*, XMLConfig&);
	public:
		BackwardCompatibleSensorFactoryImpl(XMLConfigurer configurer,
				const std::string& xmlName, const std::string& rosInterfaceXmlName):
			config(new XMLConfig()),
			configurer(configurer),
			xmlName(xmlName),
			rosInterfaceXmlName(rosInterfaceXmlName){};

		ROSInterfacePtr	createRosInterface(ros::NodeHandle nh, ROSInterfaceInfo* info, UWSimHook::Ptr hook) = 0;

		void createSensor(UWSimHook::Ptr hook) = 0;

		std::string getRosInterfaceXmlName(){return rosInterfaceXmlName;};
		std::string getSensorXmlName(){return xmlName;};

		plugin::SensorXML::Ptr getGenericSensorXML()
		{
			plugin::SensorXML::Ptr ptr(new plugin::SensorXML());
			ptr->link = config->link;
			ptr->linkName = config->linkName;
			ptr->name = config->name;

			memcpy(ptr->orientation,config->orientation,sizeof(ptr->orientation));
			memcpy(ptr->position, config->position, sizeof(ptr->position));

			return ptr;
		}

		void setGenericSensorXML(plugin::SensorXML::Ptr ptr)
		{
			config->link = ptr->link;
			config->linkName = ptr->linkName;
			config->name = ptr->name;

			memcpy(config->orientation,ptr->orientation,sizeof(ptr->orientation));
			memcpy(config->position,ptr->position, sizeof(ptr->position));
		}

		std::string getSensorSceneName(){return config->name;};

		void configureSensor(const xmlpp::Node* node)
		{
			configurer(node, *config);
		}

	protected:
		boost::shared_ptr<Sensor> sensor;
		boost::shared_ptr<XMLConfig> config;
		XMLConfigurer configurer;
		std::string xmlName, rosInterfaceXmlName;
	};

	template <class ROSIface>
	class BackwardCompatibleROSInterfaceFactoryImpl : public virtual ROSInterfaceFactory
	{
	public:
		BackwardCompatibleROSInterfaceFactoryImpl(const std::string& rosInterfaceXmlName):
			rosInterfaceXmlName(rosInterfaceXmlName){};

		ROSInterfacePtr	createRosInterface(ros::NodeHandle nh, ROSInterfaceInfo* info, UWSimHook::Ptr hook) = 0;
		std::string getRosInterfaceXmlName(){return rosInterfaceXmlName;};

	protected:
		std::string rosInterfaceXmlName;
	};
};

//Define a creator: factoryName, sensor class, xml config class, ros inteface class, xml configuration function, xml name, ros xml name,
//sensor constructor sequence
#define 	PP_LABUST_MAKE_SENSORFACTORY(FN,	S, XMLC, ROSI, XMLF, XMLN, ROSN, SCSEQ)\
	class FN : public virtual plugin::BackwardCompatibleSensorFactoryImpl<S,XMLC,ROSI> \
	{\
	protected:\
		typedef plugin::BackwardCompatibleSensorFactoryImpl<S,XMLC,ROSI> Base;\
	public: \
		FN():Base(XMLF,XMLN,ROSN){}\
		void createSensor(plugin::UWSimHook::Ptr hook) \
		{\
			sensor.reset(new S(BOOST_PP_SEQ_ENUM(SCSEQ))); \
		}	\
		ROSInterfacePtr	createRosInterface(ros::NodeHandle nh, ROSInterfaceInfo* info, plugin::UWSimHook::Ptr hook) \
		{ \
			ROSInterfacePtr ptr = boost::shared_ptr<ROSI>(new ROSI(sensor.get(),info->topic, info->rate)); \
			ptr->nh_ = nh; \
			return ptr;\
		}\
	};\

#define 	PP_LABUST_MAKE_SENSORFACTORY_EXT(FN,	S, XMLC, ROSI, XMLF, XMLN, ROSN, SCSEQ, RISEQ)\
	class FN : public virtual plugin::BackwardCompatibleSensorFactoryImpl<S,XMLC,ROSI> \
	{\
	protected:\
		typedef plugin::BackwardCompatibleSensorFactoryImpl<S,XMLC,ROSI> Base;\
	public: \
		FN():Base(XMLF,XMLN,ROSN){}\
		void createSensor(plugin::UWSimHook::Ptr hook) \
		{\
			sensor.reset(new S(BOOST_PP_SEQ_ENUM(SCSEQ))); \
		}	\
		ROSInterfacePtr	createRosInterface(ros::NodeHandle nh, ROSInterfaceInfo* info, plugin::UWSimHook::Ptr hook) \
		{ \
			ROSInterfaceInfo& rosInterface = *info; \
			ROSInterfacePtr ptr = boost::shared_ptr<ROSI>(new ROSI(BOOST_PP_SEQ_ENUM(RISEQ))); \
			ptr->nh_ = nh; \
			return ptr;\
		}\
	};\

#define 	PP_LABUST_MAKE_ROSINTERFACEFACTORY(FN, ROSI, ROSN, RISEQ)\
	class FN : public virtual plugin::BackwardCompatibleROSInterfaceFactoryImpl<ROSI> \
	{\
	protected:\
		typedef plugin::BackwardCompatibleROSInterfaceFactoryImpl<ROSI> Base;\
	public: \
		FN():Base(ROSN){}\
		ROSInterfacePtr	createRosInterface(ros::NodeHandle nh, ROSInterfaceInfo* info, plugin::UWSimHook::Ptr hook) \
		{ \
			ROSInterfaceInfo& rosInterface = *info; \
			ROSInterfacePtr ptr = boost::shared_ptr<ROSI>(new ROSI(BOOST_PP_SEQ_ENUM(RISEQ))); \
			ptr->nh_ = nh; \
			return ptr;\
		}\
	};\

#endif /* BACKWARDCOMPATIBILITY_HPP_ */
